#!/usr/bin/env python
# -*- coding:utf-8 -*-
import binascii
import datetime
# 如果没有serial这个库，要安装pyserial
import struct
import queue
import threading

import serial
import time

import matplotlib.pyplot as plt
import logging

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(message)s')
logger = logging.getLogger(__name__)


# from my_comprehensive_exp1 import Global
class Global_Var:
    wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    # 0: queue, 1: file
    data_transfer_mode = 1


_writeF1 = None


def Task_six_axis_force(queue_IHand):
    global Global_Var
    try:
        # global Global
        # COMnum = "COM" + str(COM)
        portx = "COM8"  # in Windows it maybe COM15 or  COM3 // in linux it maybe /dev/ttyUSB0 or /dev/ttyUSB1
        bps = 460800
        ser = serial.Serial(portx, bps)
        if ser.isOpen():
            print("open success")
        readed_data = bytearray()
        data_to_deal = bytearray()
        # Global.wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        # now = time.strftime("%Y%m%d%H%M%S", time.localtime(time.time()))
        # fname = now + r"tran.csv"
        # f = open(fname, "w")
        serialsendcommand = [0x48, 0xAA, 0x0D, 0x0A]  # 此处2.7和3版本写法不同
        serialsendcommandhex = serialsendcommand
        # serialsendcommandHex = binascii.b2a_hex(serialsendcommand)
        # serialsendcommandhex = serialsendcommandHex.decode("hex")
        ser.write(serialsendcommandhex)
        while True:

            # send command
            # ser.write(serialsendcommandhex)
            # read and push to buffer
            count = ser.inWaiting()
            if count > 0:
                data = ser.read(count)
                datalist = bytearray(data)
                for i in range(count):
                    readed_data.append(datalist[i])
            # deal the buffer
            while len(readed_data) > 27:
                readed_data_len = len(readed_data)
                if (readed_data_len >= 28) and (readed_data[26] == 0x0d) and (readed_data[27] == 0x0a):
                    # for i in range(0,28):
                    # data_to_deal.append(readed_data.pop(0)) #////////////////
                    data_to_deal = bytearray()
                    for i in range(0, 4):
                        data_to_deal.append(readed_data[5 - i])
                    Global_Var.wrench[0] = struct.unpack("!f", data_to_deal)[0]
                    data_to_deal = bytearray()
                    for i in range(0, 4):
                        data_to_deal.append(readed_data[9 - i])
                    Global_Var.wrench[1] = struct.unpack("!f", data_to_deal)[0]
                    data_to_deal = bytearray()
                    for i in range(0, 4):
                        data_to_deal.append(readed_data[13 - i])
                    Global_Var.wrench[2] = struct.unpack("!f", data_to_deal)[0]
                    data_to_deal = bytearray()
                    for i in range(0, 4):
                        data_to_deal.append(readed_data[17 - i])
                    Global_Var.wrench[3] = struct.unpack("!f", data_to_deal)[0]
                    data_to_deal = bytearray()
                    for i in range(0, 4):
                        data_to_deal.append(readed_data[21 - i])
                    Global_Var.wrench[4] = struct.unpack("!f", data_to_deal)[0]
                    data_to_deal = bytearray()
                    for i in range(0, 4):
                        data_to_deal.append(readed_data[25 - i])
                    Global_Var.wrench[5] = struct.unpack("!f", data_to_deal)[0]
                    for i in range(0, 28):
                        readed_data.pop(0)
                    print(Global_Var.wrench[5])  # This is the F/T Data
                    # Qtorch.put(Global.wrench[5])
                    # Qtorch
                    # Global.wrench
                    # f.write(' '.join(str(i) for i in Global.wrench) + "\n")
                elif readed_data_len >= 28:
                    if readed_data[0] == 0x0a:
                        readed_data.pop(0)
                    else:
                        i = 0
                        while (i <= 28) and (readed_data[0] != 0x0d) and (readed_data[1] != 0x0a):
                            readed_data.pop(0)
                            i = i + 1
                        readed_data.pop(0)
                        readed_data.pop(0)
                # elif(readed_data_len>=81):
                # for i in range(readed_data_len):
                # readed_data.pop(0)

                # time.sleep(0.005)
                queue_IHand.put(Global_Var.wrench[5])
                # logger.info("wrench: " + str(Global_Var.wrench[5]))

    except Exception as e:
        print("forceSensor_Error：", e)


def startRecord():
    """
    开始记录数据  Start recording data
    :return:
    """
    global _writeF1
    _writeF1 = open("./record_data/实验记录tourch" + str(datetime.datetime.now().strftime('%Y%m%d%H%M%S')) + ".txt", "w")  # 新建一个文件

    Tempstr = "Chiptime\ttourch"
    Tempstr += "\r\n"
    _writeF1.write(Tempstr)
    print("开始记录数据")


def updatetime():
    global start_time, global_chiptime
    current_time = time.time()  # 获取当前时间
    elapsed_time = current_time - start_time  # 计算经过的时间
    global_chiptime = round(elapsed_time * 1000, 1)  # 转换为毫秒


def update_data(queue_IHand):
    """
    更新数据  Update data
    :return:
    """
    global _writeF1
    while True:
        Tempstr = str(time.time())
        Tempstr += "\t"
        Tempstr += str(queue_IHand.get())
        Tempstr += "\r\n"
        _writeF1.write(Tempstr)


def stopRecord():
    """
    停止记录数据  Stop recording data
    :return:
    """
    global _writeF1
    _writeF1.close()


def mainProcess_Task_six_axis_force():
    #创建队列
    logger.info("创建队列")
    queue_IHand = queue.Queue()
    startRecord()
    thread_task1 = threading.Thread(target=Task_six_axis_force, args=(queue_IHand,))
    thread_task2 = threading.Thread(target=update_data, args=(queue_IHand,))

    try:
        thread_task1.start()
        thread_task2.start()

        # 等待线程结束
        thread_task1.join()
        thread_task2.join()
    except KeyboardInterrupt:
        logger.info("进程被终止，正在关闭文件...")
    except Exception as e:
        logger.error(f"错误：{e}")
    finally:
        # 确保文件被关闭
        stopRecord()
        logger.info("文件已关闭。")


if __name__ == '__main__':
    mainProcess_Task_six_axis_force()
